//
// Created by ZhaoXiaoFei on 2022/8/18.
//

#include "front_end/front_end.hpp"

LocalizationFrontEnd::LocalizationFrontEnd(ros::NodeHandle& nh){
    imuSub_ = nh.subscribe("/imu/data", 10, &LocalizationFrontEnd::imuCallBack, this);
    gpsSub_ = nh.subscribe("/fix", 10, &LocalizationFrontEnd::gpsCallBack, this);
    magSub_ = nh.subscribe("/imu/mag", 10, &LocalizationFrontEnd::magCallBack, this);

    fusedPosePub_ = nh.advertise<geometry_msgs::Pose>("/fused_pose", 10);
    fusedPathPub_ = nh.advertise<nav_msgs::Path>("/fused_path", 10);
    fusedPixPub_ = nh.advertise<sensor_msgs::NavSatFix>("/fused_fix",10);
    fusedOdomPub_ = nh.advertise<nav_msgs::Odometry>("/fused_odom",10);

    double acc_noise, gyr_noise, acc_bias_noise, gyr_bias_noise;
    nh.param("acc_noise", acc_noise, 1e-2);
    nh.param("gyr_noise", gyr_noise, 1e-4);
    nh.param("acc_bias_noise", acc_bias_noise, 1e-6);
    nh.param("gyr_bias_noise", gyr_bias_noise, 1e-6);

    localizationBackEnd_ = std::make_unique<LocalizationBackEnd>(acc_noise, gyr_noise, acc_bias_noise, gyr_bias_noise);
}

void LocalizationFrontEnd::imuCallBack(const sensor_msgs::Imu& imu_msg){
//        LOG(INFO) << imu_msg.linear_acceleration.x << " " <<
//                        imu_msg.linear_acceleration.y << " " <<
//                        imu_msg.linear_acceleration.z << std::endl;

    ImuData imuData;
    imuData.timestamp = ros::Time::now().toSec();
    imuData.acc = Eigen::Vector3d(imu_msg.linear_acceleration.x,
                                  imu_msg.linear_acceleration.y,
                                  imu_msg.linear_acceleration.z);
    imuData.gyr = Eigen::Vector3d(imu_msg.angular_velocity.x,
                                  imu_msg.angular_velocity.y,
                                  imu_msg.angular_velocity.z);
    localizationBackEnd_->processImuData(imuData);
    publishState();
}

void LocalizationFrontEnd::gpsCallBack(const sensor_msgs::NavSatFix& gps_msg){

    GpsPositionData gpsPositionData;
    gpsPositionData.timestamp = ros::Time::now().toSec();
    gpsPositionData.lla = Eigen::Vector3d(gps_msg.latitude,
                                          gps_msg.longitude,
                                          gps_msg.altitude);
    gpsPositionData.cov = Eigen::Map<const Eigen::Matrix3d>(gps_msg.position_covariance.data());

    localizationBackEnd_->processGpsData(gpsPositionData);
    publishState();
}

void LocalizationFrontEnd::magCallBack(const sensor_msgs::MagneticField& mag_mag){
//        LOG(INFO) << mag_mag.magnetic_field.x << " " <<
//                        mag_mag.magnetic_field.y << " " <<
//                        mag_mag.magnetic_field.z << std::endl;
    MagData magData;
    magData.timestamp = ros::Time::now().toSec();
    magData.mag = Eigen::Vector3d(mag_mag.magnetic_field.x,
                                  mag_mag.magnetic_field.y,
                                  mag_mag.magnetic_field.z);

    localizationBackEnd_->processMagData(magData);
    publishState();
}

void LocalizationFrontEnd::addStateToPath(State* state){
    ros_path_.header.frame_id = "world";
    ros_path_.header.stamp = ros::Time::now();

    geometry_msgs::PoseStamped pose;
    pose.header = ros_path_.header;

    pose.pose.position.x = state->pose_(0,3);
    pose.pose.position.y = state->pose_(1,3);
    pose.pose.position.z = state->pose_(2,3);

    const Eigen::Quaterniond q(state->pose_.block<3,3>(0,0));
    pose.pose.orientation.x = q.x();
    pose.pose.orientation.y = q.y();
    pose.pose.orientation.z = q.z();
    pose.pose.orientation.w = q.w();
//        LOG(INFO) << "Q: " << q << " P: " << state->pose_.block<3, 1>(0, 3).transpose() << std::endl;
    ros_path_.poses.push_back(pose);
}

void LocalizationFrontEnd::publishState(){
    //publish fused states
    fusedPosePub_.publish(localizationBackEnd_->getFusedPose());
    fusedOdomPub_.publish(localizationBackEnd_->getFusedOdometry());
    fusedPixPub_.publish(localizationBackEnd_->getFusedFix());
    addStateToPath(localizationBackEnd_->getState());
    fusedPathPub_.publish(ros_path_);

//        publish transform
    State* state = localizationBackEnd_->getState();
    tf::Transform tf;
    tf.setOrigin(tf::Vector3(state->pose_(0,3),state->pose_(1,3),state->pose_(2,3)));
    Eigen::Quaterniond quat(state->pose_.block<3,3>(0,0));
    tf::Quaternion q(quat.x(),quat.y(),quat.z(),quat.w());//Caution! tf::Quaternion is in (x,y,z,w)
    tf.setRotation(q);
    br_.sendTransform(tf::StampedTransform(tf,ros::Time::now(),"world","XIMU"));
}